#include <ximera/physics/ode/world.hpp>
#include <ximera/physics/ode/space.hpp>
#include <ximera/physics/ode/rigidbody.hpp>

namespace ximera { namespace physics { namespace ode
{
    namespace 
    {
        int const MAX_CONTACTS = 30;
        Real const STEP_SIZE = 0.02f;
    }

    World::World()
        : _worldID(0),
          _stepType(StepType::Quick),
          _timeStep(0)
    {
        _worldID = dWorldCreate();
        _contactGroup = dJointGroupCreate(0);

        // Create default space
        createSpace(SpaceType::Simple);
		
        // Default parameters
		dWorldSetCFM(_worldID, 1e-5f);
        //dWorldSetCFM(_worldID, 0.001f);
        //dWorldSetERP(_worldID, 0.95f);
		dWorldSetAutoDisableFlag(_worldID, 1);
		dWorldSetContactMaxCorrectingVel(_worldID, 0.1f);
		dWorldSetContactSurfaceLayer(_worldID, 0.001f);
        //dWorldSetQuickStepNumIterations(_worldID, 10);
	}

    World::~World()
    {
        _spaces.clear();

        dJointGroupDestroy(_contactGroup);
        dWorldDestroy(_worldID);
    }

	math::Vector3 World::gravity() const
	{
        math::Vector3 result;
        dWorldGetGravity(_worldID, result.array());
        return result;
	}

	void World::gravity(math::Vector3 const& value)
	{
        dWorldSetGravity(_worldID, value.x, value.y, value.z);
	}

	ISpace* World::createSpace(SpaceType type)
	{
        Space* space = new Space(this, type);
        _spaces.push_back(SharedPtr<Space>(space));

		return space;
	}

	ISpace* World::space() const
	{
		return space(0);
	}

	ISpace* World::space(UInt32 index) const
	{
		return _spaces[index].get();
	}

	UInt32 World::spaceCount() const
	{
		return (UInt32)_spaces.size();
	}

    StepType World::stepType() const
    {
        return _stepType;
    }

    void World::stepType(StepType value)
    {
        _stepType = value;
    }

	void World::step(Real secs)
	{
        _timeStep += secs;
        if (_timeStep < STEP_SIZE)
            return;

        while (_timeStep >= STEP_SIZE)
        {
            _timeStep -= STEP_SIZE;

            for (SpaceContainer::iterator it = _spaces.begin(); it != _spaces.end(); ++it)
            {
                Space* space = it->get();
                dSpaceCollide(space->spaceID(), this, &collideCallback);
            }

            switch (_stepType)
            {
            case StepType::Normal:
                dWorldStep(_worldID, STEP_SIZE);
                break;

            case StepType::Quick:
                dWorldQuickStep(_worldID, STEP_SIZE);
                break;
            }

            dJointGroupEmpty(_contactGroup);            
        }
	}

    void World::collide(dGeomID geom1, dGeomID geom2)
    {
        if (dGeomIsSpace(geom1) || dGeomIsSpace(geom2))
        {
            dSpaceCollide2(geom1, geom2, this, &collideCallback);
            return;
        }

        dBodyID body1 = dGeomGetBody(geom1);
        dBodyID body2 = dGeomGetBody(geom2);

        if (body1 && body2 && dAreConnectedExcluding(body1, body2, dJointTypeContact))
            return;

        if (body1 || body2)
        {
            collide(geom1, body1, geom2, body2);
        }
    }

    void World::collide(dGeomID geom1, dBodyID body1, dGeomID geom2, dBodyID body2)
    {
        RigidBody* rigidBody1 = body1 ? static_cast<RigidBody*>(dBodyGetData(body1)) : 0;
        RigidBody* rigidBody2 = body2 ? static_cast<RigidBody*>(dBodyGetData(body2)) : 0;

        if (rigidBody1)
        {
            if (rigidBody1->isFlagSet(BodyFlag::NoCollision))
                return;
            else if (rigidBody1->isFlagSet(BodyFlag::NoResponse))
                body1 = 0;
        }

        if (rigidBody2)
        {
            if (rigidBody2->isFlagSet(BodyFlag::NoCollision))
                return;
            else if (rigidBody2->isFlagSet(BodyFlag::NoResponse))
                body2 = 0;
        }

        if (!body1 && !body2)
            return;

        dContact contacts[MAX_CONTACTS];
        int count = dCollide(geom1, geom2, MAX_CONTACTS, &contacts[0].geom, sizeof(dContact));

        for (int i = 0; i < count; ++i)
        {
            dContact& contact = contacts[i];

            contact.surface.mode = 
                //dContactSoftERP | dContactSoftCFM | dContactBounce | dContactApprox1 /*| dContactMu2*/;
                dContactSoftCFM | dContactBounce | dContactMu2;

            //contact.surface.mu = 0.15f;
            contact.surface.mu = dInfinity;
            contact.surface.mu2 = 0.7f;
            contact.surface.slip1 = 0.1f;
            contact.surface.slip2 = 0.1f;
            contact.surface.motion1 = 0;
            contact.surface.motion2 = 0;
            contact.surface.soft_erp = 0.4f;
            contact.surface.soft_cfm = 0.001f;
            contact.surface.bounce = 0.6f;
            contact.surface.bounce_vel = 0.1f;

            dJointID contactJoint = dJointCreateContact(_worldID, _contactGroup, &contact);
            dJointAttach(contactJoint, body1, body2);
        }
    }

    void collideCallback(void* data, dGeomID geom1, dGeomID geom2)
    {
        static_cast<World*>(data)->collide(geom1, geom2);
    }
}}}
